package edu.gatech.cc.liam.geometry.linear;
 
import java.util.Arrays;
import java.util.Collection;
import java.util.HashSet;

import edu.gatech.cc.liam.core.Globals;
import edu.gatech.cc.liam.geometry.linear.convexhull.QHull;

public class PointSet extends HashSet<NPoint> {

	//static public final double APPROXBOUND = 0.95;

	/**
	 * 
	 */
	private static final long serialVersionUID = 4345972470670506029L;
	
//	private static int numQHull = 0;
//	private static int numQHullAtemp = 0;
	
	//private int maxNumPoints = 30;
	
	private int dimension = 0;
	
	public PointSet(NPoint aPoint) {
		super();
		this.add(aPoint);
		dimension = aPoint.values.length;
	}
	public PointSet(Double[] aPoint) {
		super();
		this.add(new NPoint(aPoint));
		dimension = aPoint.length;
	}
	public PointSet(double[] aPoint) {
		super();
		this.add(new NPoint(aPoint));
		dimension = aPoint.length;
	}
	public PointSet(double[][] points) {
		super();
		for(int i=0; i<points.length; i++) {
			this.add(new NPoint(points[i]));
		}
		dimension = points[0].length;
	}
	public PointSet(float[][] points) {
		super();
		for(int i=0; i<points.length; i++) {
			this.add(new NPoint(points[i]));
		}
		dimension = points[0].length;
	}
	public PointSet(Collection<? extends NPoint> c) {
		super(c);
		this.dimension = c.iterator().next().values.length;
	}
	public PointSet(){
		super();
		//this.dimension = dimension;
	}
	
	public void translate(NPoint vector) {
		for(NPoint aPoint : this) {
			aPoint.add(vector);
		}
	}
	
	public void scale(Double scaleFactor) {
		for(NPoint aPoint : this) {
			aPoint.scale(scaleFactor);
		}
	}

	public int dimensions() {
		if(this.dimension == 0 && !this.isEmpty())
			dimension = this.iterator().next().values.length;
		return dimension;
	}
	
//	private static void callQHullIfTooBig(FeasibleSet destination, FeasibleSet inputSet) {
//		numQHullAtemp++;
//		if(inputSet.size() > inputSet.maxNumPoints) {
//			numQHull++;
//			if(numQHull % 100 == 0)
//				System.out.print(" numQHullAtemp:" + numQHullAtemp + " with QHULL:" + numQHull + " ");
//			QHull.qConvexHull(destination, inputSet);
//			//destination.maxNumPoints = destination.size() * 4;
//		} else {
//			Set<FeasiblePoint> dup = new HashSet<FeasiblePoint>();
//			dup.addAll(inputSet);
//			destination.clear();
//			destination.addAll(dup);
//		}
//	}
	
//	static private void forceQHull(PointSet destination, PointSet inputSet) {
//		numQHullAtemp++;
//		numQHull++;
////		QHull.qConvexHullExact(destination, inputSet);
//		QHull.qConvexHull(destination, inputSet, APPROXBOUND);
//	}

	public void removeInteriorPoints() {
		removeInteriorPoints(Globals.TOLERANCE);
//		numQHullAtemp++;
//		numQHull++;
//		QHull.qConvexHullExact(this, this);
	}
	public void removeInteriorPoints(double approx) {
//		numQHullAtemp++;
//		numQHull++;
		QHull.qConvexHull(this, this, approx);
	}
	
//	private void approximateHullWithHyperSet(ApproximatingPolytope approxHull) {
//		approxHull.approximateSet(this);
//		
//	}
	
//	private void union(FeasibleSet otherSet) {
//		this.addAll(otherSet);
//		forceQHull(this, this);
//	}

//	@Override
//	public boolean addAll(Collection<? extends FeasiblePoint> c) {
//		return false; //unionAll(c)
//	}
	
	public void unionAll(Collection<PointSet> otherSets) {
		for(PointSet aSet : otherSets) {
			this.addAll(aSet);
		}
	}
	public void union(PointSet otherSet) {
		this.addAll(otherSet);
	}
	
	public void minkowskiSum(Collection<NPoint> aSet) {
		crossSum_Brute(aSet);
	}
	
	private void crossSum_Brute(Collection<NPoint> aSet) {
		//TODO Make more efficient
		PointSet newSet = new PointSet();
//		if(this.size() > this.maxNumPoints || aSet.size() > this.maxNumPoints ) {
//			this.forceQHull();
//			aSet.forceQHull();
//		}
		for(NPoint point1 : this) {
			for(NPoint point2 : aSet) {
				newSet.add(NPoint.sum(point1, point2));
			}
		}
		this.clear();
		this.addAll(newSet);
		this.removeInteriorPoints();
//		this.forceQHull(this, newSet);
//		callQHullIfTooBig(this, newSet);
	}
	
	public boolean softContains(NPoint containPoint, double tolerance) {
		for(NPoint aPoint : this) {
			if(aPoint.softEquals(containPoint, tolerance))
				return true;
		}
		return false;
	}
	
	// checks for a soft 1-1 correspondence of vertices
	public boolean softEquals(PointSet otherSet) {
		return softEquals(otherSet, Globals.TOLERANCE);
	}
	// checks for a soft 1-1 correspondence of vertices
	public boolean softEquals(PointSet otherSet, double tolerance) {
		for(NPoint aPoint : this) {
			if(! otherSet.softContains(aPoint, tolerance))
				return false;
		}
		for(NPoint aPoint : otherSet) {
			if(! this.softContains(aPoint, tolerance))
				return false;
		}
		return true;
	}
	
	// incorrect hausdorff Distance. Does not consider in between vertexes.
	@Deprecated
	public double hausdorffDistanceBad(PointSet outerSet) {
		double maxmin = Double.MIN_VALUE;
		for(NPoint pointA : outerSet) {
			double min = Double.MAX_VALUE;
			for(NPoint pointB : this) {
				min = Math.min(min, pointA.distance(pointB));
			}
			maxmin = Math.max(maxmin, min);
		}
		return maxmin;
	}

	/**
	 * @return A set of normals (as points) such that any linear combination of points will
	 * form a normal of a hyperplane that passes through all points.
	 * For example, if the points uniquely define a hyperplane, then this function will return
	 * exactly two points (the negative and positive directions)
	 * If no hyperplane can satisfy all points, then null is returned.
	 */
	protected PointSet getPossibleHalfspacesFromPoints() {
		NPoint[] pts = new NPoint[this.size()];
		this.toArray(pts);
		//if there are too few points, or too many collinear points then the number of feasible normals will be large
		//if there are too many linearly independent points, then the resulting equations will be infeasible
		//assert pts.length == pts[0].dimensions();
		int dimensions =  pts[0].dimensions();
		Polytope dualPolytope = new Polytope();
		for(int pNum=0; pNum<pts.length; pNum++) {
			double[] normal = Arrays.copyOf(pts[pNum].values, dimensions+1);
			normal[dimensions] = 1.0;
			dualPolytope.addEqualityConstraint(new Halfspace(normal, 0.0));
		}
		for(int i=0; i<dimensions; i++) {
			double[] bound1 = new double[dimensions+1]; // variable <= 1
			bound1[i] = 1.0;
			dualPolytope.add(new Halfspace(bound1, -1.0));	
			double[] bound2 = new double[dimensions+1]; // variable >= -1
			bound2[i] = -1.0;
			dualPolytope.add(new Halfspace(bound2, -1.0));	
		}
		if (! dualPolytope.isFeasible())
			return null;
		return dualPolytope.getExtremePoints();
	}
	
	/**
	 * @return a hyperplane for which all points lie on, or null if none exists
	 * The normal will have the first possible variable being greater than 0.
	 * A null value is returned if the points do not lie on a halfspace.
	 * If there are too few points, or too many linearly dependent points then the number of feasible normals will be greater than 2.
	 */
	public Polytope getManifold() {
		PointSet points = getPossibleHalfspacesFromPoints();
		Polytope rTope = new Polytope();
		for(NPoint p : points)
			rTope.add(new Halfspace(p.values));
		return rTope;//makeHalfspaceFromLowestOrthantPoint(points);
	}

	/**
	 * @return A polytope with extreme points equal to the convex hull of this set of points.
	 */
	public Polytope getConvexHull() {
		Polytope rP = new Polytope();
		QHull.qConvexHullExact(null, rP, this);
		return rP;
	}
	
	public NPoint getAverage() {
		double[] zeros = new double[dimensions()];
		NPoint center = new NPoint(zeros);
		for(NPoint aPt : this) {
			center.add(aPt);
		}
		center.scale(1.0 / this.size());
		return center;
	}
	/**
	 * @return
	 * the minimum values for each dimension
	 */
	public double[] getMinBounds() {
		double[] mins = new double[dimensions()];
		Arrays.fill(mins, Double.MAX_VALUE);
		for(NPoint p : this) {
			for(int d=0; d<p.dimensions(); d++) {
				mins[d] = Math.min(mins[d], p.values[d]);
			}
		}
		return mins;
	}
	/**
	 * @return
	 * the maximum values for each dimension
	 */
	public double[] getMaxBounds() {
		double[] maxs = new double[dimensions()];
		Arrays.fill(maxs, Double.MIN_VALUE);
		for(NPoint p : this) {
			for(int d=0; d<p.dimensions(); d++) {
				maxs[d] = Math.max(maxs[d], p.values[d]);
			}
		}
		return maxs;
	}
	
	public static PointSet makeBounds(Double[] minBounds, Double[] maxBounds, double gamma) {
		PointSet newSet = new PointSet();
		int numPts = (int) Math.pow(2, minBounds.length);
		for(int ptNum=0; ptNum<numPts; ptNum++) {
			double[] newPt = new double[minBounds.length];
			for(int playerNum=0; playerNum<minBounds.length; playerNum++) {
				boolean negate = ((ptNum>>>playerNum)%2)==1;
				if(negate)
					newPt[playerNum] = minBounds[playerNum]/(1-gamma);
				else
					newPt[playerNum] = maxBounds[playerNum]/(1-gamma);					      
			}
			//System.out.println("Bounds #"+ptNum + " = " + Arrays.toString(newPt));
			newSet.add(new NPoint(newPt));
		}
		return newSet;
	}
	
	public NPoint getFarthestPoint(NPoint center) {
		NPoint farthest = null;
		double farDist = 0.0;
		for(NPoint aPt : this) {
			double dist = center.distance(aPt);
			if(dist > farDist) {
				farDist = dist;
				farthest = aPt;
			}
		}
		return farthest;
	}
	
	@Override
	public PointSet clone() {
		PointSet newSet = new PointSet();
		for(NPoint aP : this) {
			newSet.add(aP.clone());
		}
		return newSet;
	}
	@Override
	public String toString() {
		String setStr = super.toString();
		return setStr;
	}

	public static void main(String[] args) {
		PointSet t = new PointSet();
		t.add(new NPoint( new double[]{0.0, 0.0} ));
		t.add(new NPoint( new double[]{1.0, 0.0} ));
		t.add(new NPoint( new double[]{0.0, 1.0} ));
		t.add(new NPoint( new double[]{0.2, 0.2} ));
		System.out.println(t);
		t.removeInteriorPoints();
		System.out.println("Removing interior points...");
		System.out.println(t);
	}
}
